#pragma config(Sensor, in1,    potentiometer,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  encoder,        sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  touch,          sensorTouch)
#pragma config(Motor,  port1,           leftArm1,      tmotorVex393, openLoop)
#pragma config(Motor,  port2,           rightArm2,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port3,           frontLeft,     tmotorVex393, openLoop)
#pragma config(Motor,  port4,           rightIntake,   tmotorVex393, openLoop)
#pragma config(Motor,  port5,           backLeft,      tmotorVex393, openLoop)
#pragma config(Motor,  port6,           leftArm2,      tmotorVex393, openLoop)
#pragma config(Motor,  port7,           rightArm1,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port8,           frontRight,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port9,           leftIntake,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          backRight,     tmotorVex393, openLoop, reversed)

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

const int maxSpeed = 127;
const int floorValue = 10;
const int holdSpeed = 25;
const int rotationTime180 = 1100;//880;
const int rotationTime90 = 440;
const bool teamSide = true; // true => RIGHT false => LEFT

const int circumference = 12; // inches

// Encoder Constants
const int encoderConversion = 360; // counts per wheel revolution
const float turnRevs = 0.643; // wheel revolutions per 90 degree turn

const float slant = 20;

// Potentiometer Constants
const int maxPotentiometer = 4092;
const int potentiometerConversion = 270;
const int difference = 5;

void move(int l, int r) {
	motor[backLeft] = l;
	motor[frontLeft] = l;
	motor[backRight] = r;
	motor[frontRight] = r;
}

void moveArm(int speed) {
	motor[leftArm1] = speed;
	motor[leftArm2] = speed;
	motor[rightArm1] = speed;
	motor[rightArm1] = speed;
}

void moveIntake(int speed) {
	motor[leftIntake] = speed;
	motor[rightIntake] = speed;
}

bool aligned(float angle, float speedY, float speedX) {

	// Checking Quadrant

	if (speedY > 0 && speedX > 0 && !(angle > 0 && angle < PI/2))
		return false;
	else if (speedY > 0 && speedX < 0 && !(angle > PI/2 && angle < PI))
		return false;
	else if (speedY < 0 && speedX < 0 && !(angle > PI && angle < 3*PI/2))
		return false;
	else if (speedY < 0 && speedX > 0 && !(angle > 3*PI/2 && angle < 2*PI))
		return false;

	return true;

}

float realignedAngle(float angle, float speedY, float speedX) {

	while (!aligned(angle,speedY,speedX)) {
		if (angle >= PI)
			angle -= PI;
		else
			angle += PI;
	}

	return angle;

}

void rest() {
	move(0,0);
}

void restTime(int ms) {

	move(0,0);
	wait1Msec(ms);

}

// Reset Encoder Sensor Value to 0
void clearEncoder() {

	SensorValue[encoder] = 0;

}

// Convert Distance (Inches) to Encoder Value (Counts)
int encoderValueOf(float distance) {

	return round(distance/circumference*encoderConversion);

}

// Move (inches)
void moveInches(float distance) {

	clearEncoder();

	if (distance == 0) {
		move(0,0);
		return;
	}

	if (distance > 0)
		move(maxSpeed,maxSpeed);
	else
		move(-maxSpeed,-maxSpeed);

	while(abs(SensorValue[encoder]) < abs(encoderValueOf(distance))) {}

	move(0,0);

}

// Move (inches) while Raising Arm
void moveInches(float distance, bool b) {

	clearEncoder();

	if (distance == 0) {
		move(0,0);
		return;
	}

	if (b)
		moveArm(maxSpeed);

	if (distance > 0)
		move(maxSpeed,maxSpeed);
	else
		move(-maxSpeed,-maxSpeed);

	while(abs(SensorValue[encoder]) < abs(encoderValueOf(distance))) {}

	move(0,0);

}

int potentiometerValueOf(int value) {

	return value%maxPotentiometer;

}

// Move Arm (inches)
void moveArmInches(float inches) {

	int startingValue = SensorValue[potentiometer];

	moveArm(maxSpeed);

	int count = 0;

	int rotationValue = inches*potentiometerConversion;

	int cycles = floor(rotationValue/maxPotentiometer)+1;

	int rotationValue1 = (rotationValue+startingValue)%maxPotentiometer;
	int rotationValue2 = (rotationValue+difference+startingValue)%maxPotentiometer;

  while (count != cycles) {

	  if (SensorValue[potentiometer] > rotationValue1 && SensorValue[potentiometer] < rotationValue2)
			count++;

	}

	moveArm(0);

}

void raiseArmFull() {

  moveArm(maxSpeed);
  wait1Msec(2000);
  moveArm(0);

}

void lowerArmFull() {

	moveArm(-maxSpeed);
	wait1Msec(1000);
	moveArm(0);

}

// Turn By Degrees
void turnDegrees(float degrees) {

	clearEncoder();

	if (degrees == 0)
		move(0,0);

	if (degrees > 0)
		move(maxSpeed,-maxSpeed);
	else if (degrees < 0)
		move(-maxSpeed,maxSpeed);

	while (abs(SensorValue[encoder]) < abs(encoderValueOf((degrees/90)*turnRevs*circumference))) {}

	move(0,0);

}

// Point Left Turn
void leftTurn() {

	clearEncoder();

	move(-maxSpeed,maxSpeed);

	while(abs(SensorValue[encoder]) < abs(encoderValueOf(turnRevs*circumference))) {}

	move(0,0);

}

// Point Right Turn
void rightTurn() {

	clearEncoder();

	move(maxSpeed,-maxSpeed);

	while(abs(SensorValue[encoder]) < abs(encoderValueOf(turnRevs*circumference))) {}

	move(0,0);

}

// Point 180 Turn
void turn180() {

	clearEncoder();

	move(maxSpeed,-maxSpeed);

	while(abs(SensorValue[encoder]) < abs(encoderValueOf(2*turnRevs*circumference))) {}

	move(0,0);

}

/*void followLineTime(int time) {



}

void followLineInches(int distance) {

	clearEncoder();

	int sign = 1;

	if (distance == 0) {
		move(0,0);
		return;
	} else if (distance < 0) {
		sign = -1;
	}

	while(abs(SensorValue[encoder]) < abs(encoderValueOf(distance))) {

		int l = SensorValue[leftLine];
		int r = SensorValue[rightLine];
		int m = SensorValue[middleLine];

		if (l < 400 && r < 400) {
			move(sign*(l+m)/(2*4095)*maxSpeed,sign*(r+m)/(2*4095)*maxSpeed);
		} else {
			move(sign*maxSpeed,sign*maxSpeed);
		}

	}

}*/

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
  clearEncoder();

}

void insideSquareEncoderAuton() {

	// Raise Arm
	moveArmInches(26);
	moveArm(holdSpeed);

	// Forward
	moveInches(12);
	restTime(1000);

	// Backward
	moveInches(-12);

	// Rotate
	/*if (teamSide)
		turnDegrees(-slant);
	else
		turnDegrees(slant);
	restTime(250);*/
	//restTime(2000);
	while (SensorValue[touch] == 0) {}

	// Forward
	moveInches(14);
	restTime(500);

	// Backward
	moveInches(-14);

	// Lower Arm
	lowerArmFull();

}

void insideSquareEncoderStashingAuton() {

	// Forward + Intake
	moveIntake(-maxSpeed);
	moveInches(4);
	moveInches(-4);
	moveIntake(0);

	// Raise Arm
	moveArmInches(26);
	moveArm(holdSpeed);

	// Forward
	moveInches(12);
	restTime(250);

	// Backward
	moveInches(-12);

	// Rotate
	/*if (teamSide)
		turnDegrees(-slant);
	else
		turnDegrees(slant);
	restTime(250);*/
	//restTime(2000);
	while (SensorValue[touch] == 0) {}

	// Forward
	moveInches(26);
	restTime(500);

	// Backward
	moveInches(-26);

	// Rotate
	/*if (teamSide)
		turnDegrees(-(90-slant));
	else
		turnDegrees(90-slant);
	restTime(250);*/
	//restTime(2000);
	while (SensorValue[touch] == 0) {}

	// Lower Arm
	lowerArmFull();

	// Forward
	moveInches(36);

	// Forward + Raise Arm
	moveInches(16, true);

	moveArm(maxSpeed);
	wait1Msec(1300);

	moveInches(4);

	// Outake (Stash)
	moveIntake(maxSpeed);
	wait1Msec(2000);
	moveIntake(0);

	moveInches(-4);

}

void outsideSquareAuton() {

	// Forward + Intake
	moveIntake(-maxSpeed);
	moveInches(12);

	// Backward
	moveIntake(0);
	moveInches(-12);

	// Rotate
	raiseArmFull();
	moveIntake(maxSpeed);

	while (SensorValue[touch] == 0) {}

	moveIntake(0);
	lowerArmFull();

	// Forward
	moveInches(6);

	// Turn Left
	leftTurn();

	// Forward
	moveInches(10);

	// Turn Left
	leftTurn();

	// Forward
	moveInches(12);

}

void outsideSquareAutonOverBump() {

	// Forward + Intake
	move(maxSpeed,maxSpeed);
	moveIntake(-maxSpeed);
	wait1Msec(1200);

	// Stop Intake + Turn 180 degrees
	moveIntake(0);
	move(maxSpeed,-maxSpeed);
	wait1Msec(rotationTime180);

	// Forward + Raise Arm (for bump)
	move(maxSpeed,maxSpeed);
	moveArm(maxSpeed);
	wait1Msec(200);
	moveArm(0);
	wait1Msec(1900);

	// Release
	moveIntake(maxSpeed);
	wait1Msec(1500);

	// Stop Release + Rotate 180 degrees
	moveIntake(0);
	move(maxSpeed,-maxSpeed);
	wait1Msec(rotationTime180);

	// Forward + Raise Arm (for bump)
	move(maxSpeed,maxSpeed);
	moveArm(maxSpeed);
	wait1Msec(200);
	moveArm(0);
	wait1Msec(1900);

	// Rest (for User Adjustment)
	move(0,0);
	wait1Msec(2000);

	// Forward
	move(maxSpeed,maxSpeed);
	wait1Msec(1000);

	// Rotate 90 degrees (left/right depending on side)
	if (teamSide)
		move(-maxSpeed,maxSpeed);
	else
		move(maxSpeed,-maxSpeed);
	wait1Msec(rotationTime90);

	// Forward
	move(maxSpeed,maxSpeed);
	wait1Msec(3000);

}

void programmingskills() {

	// Move Forward + Intake
	moveIntake(-maxSpeed);
	moveInches(18);
	moveIntake(0);

	// Move Backward
	moveInches(-36);

	// Turn Left
	if (teamSide)
		rightTurn();
	else
		leftTurn();

	// Move Forward
	moveInches(18);

	// Turn Left
	if (teamSide)
		rightTurn();
	else
		leftTurn();

	// Move Forward
	moveInches(48);

	// Raise Arm
	moveArm(maxSpeed);
	wait1Msec(1000);
	moveArm(holdSpeed);

	// Outake
	moveIntake(maxSpeed);
	restTime(5000);
	moveIntake(0);

	// Lower Arm
	moveArm(-maxSpeed);
	wait1Msec(1000);
	moveArm(0);

	// Move Backwards
	moveInches(-48);

	// Turn Right
	if (teamSide)
		leftTurn();
	else
		rightTurn();

	// Move Backwards
	moveInches(-18);

	// Turn Left
	if (teamSide)
		rightTurn();
	else
		leftTurn();

	// Move Forward
	moveInches(24);

	// Move Backwards
	moveInches(-24);

	// Rotate
	/*if (teamSide)
		turnDegrees(-stashSlant);
	else
		turnDegrees(stashSlant);
	restTime(250);*/
	restTime(2000);

	// Move Forward
	moveInches(30);

	// Move Backwards
	moveInches(-30);

	// Rotate
	/*if (teamSide)
		turnDegrees(-(90-stashSlant));
	else
		turnDegrees(90-stashSlant);
	restTime(250);*/
	restTime(2000);

	moveInches(63);

	// Turn Left
	if (teamSide)
		rightTurn();
	else
		leftTurn();

	// Move Forward
	moveInches(24);

	// Move Backwards
	moveInches(-24);

	// Turn Right
	if (teamSide)
		leftTurn();
	else
		rightTurn();

	// Move Forward
	moveInches(18);

	// Turn Left
	if (teamSide)
		rightTurn();
	else
		leftTurn();

	// Move Forward
	moveInches(24);

	// Move Backwards
	moveInches(-24);

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{

	insideSquareEncoderAuton();

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{

	// Driver Program (Chassis with 2 left wheels, 2 right wheels)

	while (true) {

		// TEST 1 ENCODER ROTATION
		// TEST SLANTS

		// Ch1: Right Joystick X-Axis
		// Ch2: Right Joystick Y-Axis
		// Ch3: Left Joystick Y-Axis
		// Ch4: Left Joystick X-Axis

		// Movement (Dual Joystick)

		// Left Joystick
		float speedY = vexRT[Ch3];
		// Right Joystick
		float speedX = vexRT[Ch1];

		if (abs(speedY) > floorValue && abs(speedX) > floorValue) {

			float angle = atan(speedY/speedX);
			angle = realignedAngle(angle,speedY,speedX);

			float speed = sqrt(speedY*speedY + speedX*speedX);
			if (speed > maxSpeed)
				speed = maxSpeed;

			speedY = speed*sin(angle);
			speedX = speed*cos(angle);

			move(speedY + speedX, speedY - speedX);

		} else
			move(vexRT[Ch3] + vexRT[Ch1], vexRT[Ch3] - vexRT[Ch1]);

		// Point Turns
		if (vexRT[Btn7L] == 1)
			leftTurn();
		else if (vexRT[Btn7R] == 1)
			rightTurn();

		// Arm Motion
		if (vexRT[Btn6U] == 1 && vexRT[Btn5U] == 1)
			moveArm(holdSpeed);
		else if (vexRT[Btn6U] == 1)
			moveArm(maxSpeed);
		else if (vexRT[Btn5U] == 1)
			moveArm(-maxSpeed);
		else
			moveArm(0);

		// Intake Motion
		if (vexRT[Btn6D] == 1)
			moveIntake(maxSpeed);
		else if (vexRT[Btn5D] == 1)
			moveIntake(-maxSpeed);
		else
			moveIntake(0);

		// Autonomous
		if (vexRT[Btn7D] == 1)
			insideSquareEncoderStashingAuton();

	}

}
